﻿#ifndef PICKMOVEZ_H
#define PICKMOVEZ_H
#include "robotCommon.h"
#include "netHelper.h"
#include <string>
#include <QString>
#include <QObject>
#include <QTimer>
#include <QVector3D>
#include <QtMath>
#include <QThread>
#include <QVector>
#include <QMap>


using namespace std;

extern string currtool;
extern string currobj;
extern float jogacc;
extern float jogdacc;
extern float prospeed;
extern HelperJogMode jogmode;
extern HelperRobJointTarget* curjoint;
extern HelperRobTarget* curpos;
extern bool isconnected;
extern string robotip;
extern QMap<QString,int> rob_IO;
namespace Taike {

static Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.u0 = cy * cp * cr + sy * sp * sr;
    q.u1 = cy * cp * sr - sy * sp * cr;
    q.u2 = sy * cp * sr + cy * sp * cr;
    q.u3 = sy * cp * cr - cy * sp * sr;

    return q;
}
static QVector3D ToEulerAngles(Quaternion q) {
    QVector3D angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.u0 * q.u1 + q.u2 * q.u3);
    double cosr_cosp = 1 - 2 * (q.u1 * q.u1 + q.u2 * q.u2);
    angles.setX(std::atan2(sinr_cosp, cosr_cosp));

    // pitch (y-axis rotation)
    double sinp = 2 * (q.u0 * q.u2 - q.u3 * q.u1);
    if (std::abs(sinp) >= 1)
        angles.setY(std::copysign(M_PI / 2, sinp)); // use 90 degrees if out of range
    else
        angles.setY(std::asin(sinp));

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.u0 * q.u3 + q.u1 * q.u2);
    double cosy_cosp = 1 - 2 * (q.u2 * q.u2 + q.u3 * q.u3);
    angles.setZ(std::atan2(siny_cosp, cosy_cosp));

    return angles;
}
static QVector<double> getcurrentJoint()
{
    HelperRobJointTarget target= *curjoint;
    QVector<double> joint;
    joint<<target.rax[0]<<target.rax[1]<<target.rax[2]<<target.rax[3]<<target.rax[4]<<target.rax[5];
    return joint;
}
static QVector<double> getcurrentPos()
{
    HelperRobTarget target= *curpos;
    QVector<double> pos;
    pos<<target.pos.x<<target.pos.y<<target.pos.z<<target.orient.u0<<target.orient.u1<<target.orient.u2<<target.orient.u3;
    return pos;
}

class monitor;

class PickMoveZ:public QObject
{
    Q_OBJECT
public:
    PickMoveZ();
    static PickMoveZ* Instance(){
        if(_self==nullptr)
            _self=new PickMoveZ();
        return PickMoveZ::_self;
    };
    ~PickMoveZ();
public slots:
    void setip(QString ip);
    bool netstatus();
    void connect();
    void disconnect();
    void setenable(bool ok);
    void setEMode();
    void setEReset();
    void SetCoordSysterm(int c);
    void setjogmodel(int md);
    void setprospeed(float sp);
    void setJogAcc(float acc);
    void setJogDAcc(float dacc);
    void jogJoint(int JNum,int dir,int step = 0);
    void jogPos(int Index,int dir,int step = 0);
    void stopJog();
    void movepp(QVector<double> target);
    void moveL(QVector<double> target);
    void moveC(QVector<double> pass,QVector<double> end);
    void movestop();
    int isarrived(int index);
    void clearerror();
    void setspeed(int sp);
    int Getspeed();
    monitor* getmonitor();

    void LoadProgram(QString path);
    void setProRunMode(bool isstep);
    void getProstatus();
    int GetDI(QString name);
    void SetDO(QString name,int i);



signals:
    void networkconnected(bool iscon);
    void sig_robotstatus(QString str);
private:
    void getState();
    void checkip(const char* ip);
private slots:
    void updatepos();
    void jogslot();
    void movemonitor();
    void errorocced();

private:
    static PickMoveZ* _self;

    QTimer *timer;
    string mcurrtool=" ";
    string mcurrobj=" ";
    float mjogacc=0;
    float mjogdacc=0;
    float mprospeed=0;
    HelperJogMode mjogmode=HELPER_JOG_MODE_AXISGROUP1;
    int _dof;
    bool isinit=false;
    QTimer *jogtimer;
    int jogjointnum;
    int jogdir;
    int jogstep;
    monitor* moni;
    QTimer *evt;
    QTimer *movebeat;
    int movepointsize;
    int trynumber=0;
    SpeedData* speeddata;
    HelperExecutionMode ExecutionMode;

};
class monitor : public QThread
{
    Q_OBJECT
public:
    monitor()
    {

    }
    ~monitor()
    {

    }
signals:
    void sig_ErrorOcc();

private:
    void run() override;
};
}

#endif // PICKMOVEZ_H
